#include <math.h>
#include "commandParser.h";

/*
void* operator new(size_t size) { return malloc(size); }
void operator delete(void* ptr) { free(ptr); } 
*/
//=========================================================================================================
char buffer[100];
//char buffer[150];
int received;
//=========================================================================================================
// returns val with number of decimal places determine by precision
// NOTE: precision is 1 followed by the number of zeros for the desired number of decimial places
// example: doubleToString( 3.1415, 100); // returns 3.14 (two decimal places)
String doubleToString(double val, unsigned int precision)
{	
//	char* value;
//	sprintf(value, "%f", val);
	
	String value = String(int(val)); //  adding integer part	
	value.concat(".");//  adding  decimal point	
    unsigned int frac;
    if(val >= 0)
	  frac = (val - int(val)) * precision;
    else
	  frac = (int(val)- val ) * precision;
	 // adding leading zeros
	if (frac != 0) 
		for (int cont = frac*10;cont < precision;cont*=10)
			value.concat("0");
	value.concat(frac); // adding fractional part
	return value;
}
//=========================================================================================================
void sendMessage(void *ptr)
{
	Message *aMessage = (Message *)ptr;
	
	Serial.write(MESSAGE_START);			// write used to get the leter and not the numerical value
	Serial.print(aMessage->length);			// print used to get the numerical value
	Serial.print(aMessage->address); 		// print used to get the numerical value
	Serial.write(aMessage->command);		// write used to get the leter and not the numerical value*
	Serial.print(aMessage->dataFormat);		// print used to get the numerical value
	Serial.write((char*)aMessage->data);	// write used to get the chain of leters
	Serial.write(MESSAGE_END);				// write used to get the leter and not the numerical value

	/* set x = "typecasted" ptr to fix stupid auto-prototyping compile error*/
//   x = (Message *)ptr;
}
//=========================================================================================================
void processReceivedMessage(void *ptr)
{
	Message *aMessage = (Message *)ptr;
	if (aMessage->command == COMMAND_REQUEST_TEMPERATURE)
	{		
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_TEMPERATURE;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(CURRENT_TEMPERATURE,100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);
	}
	else if (aMessage->command == COMMAND_REQUEST_ACCELERATION_X)
	{
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_ACCELERATION_X;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(getAcc('x'),100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);	
	}
	else if (aMessage->command == COMMAND_REQUEST_ACCELERATION_Y)
	{
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_ACCELERATION_Y;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(getAcc('y'),100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);	
	}
	else if (aMessage->command == COMMAND_REQUEST_ACCELERATION_Z)
	{
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_ACCELERATION_Z;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(getAcc('z'),100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);	
	}
	else if (aMessage->command == COMMAND_REQUEST_PITCH)
	{
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_PITCH;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(getRotation('x'),100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);	
	}
	else if (aMessage->command == COMMAND_REQUEST_ROLL)
	{
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_ROLL;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(getRotation('y'),100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);	
	}
	else if (aMessage->command == COMMAND_REQUEST_YAW)
	{
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_YAW;
		aMessage->dataFormat = 1;
		String dataString = doubleToString(getRotation('z'),100);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);	
	}
	else if (aMessage->command == COMMAND_REQUEST_PARAMETER)
	{		
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_RESPONSE_PARAMETER;
		aMessage->dataFormat = 1;
		String dataString = getParameterValue(aMessage->data[0]);
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);	
	}
	else if (aMessage->command == COMMAND_SET_PARAMETER)
	{		
		aMessage->address 	 = 1;
		aMessage->command 	 = COMMAND_ACKNOWLEDGE;
		aMessage->dataFormat = 1;
		int parameter = aMessage->data[0];
		aMessage->data[0] = '0';
		int setResult = setParameterValue(parameter,(char*)aMessage->data);
		if (setResult != 0)
			aMessage->command 	 = COMMAND_ERROR;
		String dataString;
		dataString.toCharArray((char*)aMessage->data,100);
		aMessage->length  	 = sizeof(dataString) + 6;
		sendMessage(aMessage);	
	}
	/*
	if (aMessage->command == COMMAND_ACKNOWLEDGE)
	{			
		digitalWrite(RED_ALARM_LED2, HIGH); // turn the LED on
		delay(2000);
		digitalWrite(RED_ALARM_LED2, LOW); 
	} */  
}
//=========================================================================================================
void readCommands()
{
/*	if (Serial.available())
    {
	  buffer[received++] = Serial.read();
	  buffer[received] = '\0';
	  if (received >= (sizeof(buffer)-1))
	  {
		Serial.print(buffer);
		received = 0;
	  }
    }
*/
	if (Serial.available())
    {
		byte pieceOfData = Serial.read();
		if ( pieceOfData == MESSAGE_START)
		{
				received = 0;
		}
		else if ( pieceOfData == MESSAGE_END)
		{
			Message aMessage;
			aMessage.length  	= buffer[0];
			aMessage.address 	= buffer[1];
			aMessage.command 	= buffer[2];
			aMessage.dataFormat = buffer[3];
			
			for (int cont = 4;cont < sizeof(buffer);cont++ )
			{
				aMessage.data[cont-4] = buffer[cont];
			}
			Serial.println(buffer);
			processReceivedMessage(&aMessage);
		}
		else
		{
			buffer[received++] = pieceOfData;
			buffer[received] = '\0';
		}

	}	
}
//=========================================================================================================
String getParameterValue(byte parameter)
{
//			Serial.println("+================================");
//			Serial.println(UPPER_TEMPERATURE_THRESHOLD);
	String parameterValue = "";
	switch (parameter)
	{
		case PARAMATER_UPPER_TEMPERATURE_THRESHOLD:  
			parameterValue = doubleToString(UPPER_TEMPERATURE_THRESHOLD,100);		
		break;
		case PARAMATER_LOWER_TEMPERATURE_THRESHOLD:  
			parameterValue = doubleToString(LOWER_TEMPERATURE_THRESHOLD,100);		
		break;		
		case PARAMATER_SUDEN_CHANGE_TEMPERATURE_THRESHOLD:
			parameterValue = doubleToString(SUDEN_CHANGE_TEMPERATURE_THRESHOLD,100);		
		break;
		case PARAMATER_TOTAL_ACCELERATION_ALARM_THRESHOLD:
			parameterValue = doubleToString(TOTAL_ACCELERATION_ALARM_THRESHOLD,100);		
		break;
		case PARAMATER_MAXIMUM_TEMPERATURE:
			parameterValue = doubleToString(MAXIMUM_TEMPERATURE,100);		
		break;
		case PARAMATER_MINIMUM_TEMPERATURE:
			parameterValue = doubleToString(MINIMUM_TEMPERATURE,100);		
		break;

		case PARAMATER_MAXIMUM_ACCELERATION_X:
			parameterValue = doubleToString(MAXIMUM_ACCELERATION_X,100);		
		break;
		case PARAMATER_MAXIMUM_ACCELERATION_Y:
			parameterValue = doubleToString(MAXIMUM_ACCELERATION_Y,100);		
		break;
		case PARAMATER_MAXIMUM_ACCELERATION_Z:	
			parameterValue = doubleToString(MAXIMUM_ACCELERATION_Z,100);		
		break;
		
		case PARAMATER_MINIMUM_ACCELERATION_X:	
			parameterValue = doubleToString(MINIMUM_ACCELERATION_X,100);		
		break;
		case PARAMATER_MINIMUM_ACCELERATION_Y:	
			parameterValue = doubleToString(MINIMUM_ACCELERATION_Y,100);		
		break;
		case PARAMATER_MINIMUM_ACCELERATION_Z:	
			parameterValue = doubleToString(MINIMUM_ACCELERATION_Z,100);		
		break;
		
		case PARAMATER_AUTOMATIC_TEMPERATURE_TRANSMISION_PERIOD:
			parameterValue = doubleToString(AUTOMATIC_TEMPERATURE_TRANSMISION_PERIOD,100);		
		break;
		case PARAMATER_AUTOMATIC_TEMPERATURE_TRANSMISION_THRESHOLD:
			parameterValue = doubleToString(AUTOMATIC_TEMPERATURE_TRANSMISION_THRESHOLD,100);		
		break;
		case PARAMATER_AUTOMATIC_TOTAL_ACCELERATION_PERIOD:
			parameterValue = doubleToString(AUTOMATIC_TOTAL_ACCELERATION_PERIOD,100);		
		break;
		case PARAMATER_AUTOMATIC_TOTAL_ACCELERATION_THRESHOLD:
			parameterValue = doubleToString(AUTOMATIC_TOTAL_ACCELERATION_THRESHOLD,100);		
		break;
	}
	return parameterValue;
}

//=========================================================================================================
int setParameterValue(byte parameter, char* parameterValue)
{
//			Serial.println("+================================");
//			Serial.println(parameterValue);			
//			Serial.println(UPPER_TEMPERATURE_THRESHOLD);
	int valueSetSuccessfully = 0;
	switch (parameter)
	{
		case PARAMATER_UPPER_TEMPERATURE_THRESHOLD:  
			UPPER_TEMPERATURE_THRESHOLD = atof(parameterValue);		
		break;
		case PARAMATER_LOWER_TEMPERATURE_THRESHOLD:  
			LOWER_TEMPERATURE_THRESHOLD = atof(parameterValue);		
		break;		
		case PARAMATER_SUDEN_CHANGE_TEMPERATURE_THRESHOLD:
			SUDEN_CHANGE_TEMPERATURE_THRESHOLD = atof(parameterValue);		
		break;
		case PARAMATER_TOTAL_ACCELERATION_ALARM_THRESHOLD:
			TOTAL_ACCELERATION_ALARM_THRESHOLD = atof(parameterValue);		
		break;
		case PARAMATER_MAXIMUM_TEMPERATURE:
			MAXIMUM_TEMPERATURE = atof(parameterValue);		
		break;
		case PARAMATER_MINIMUM_TEMPERATURE:
			MINIMUM_TEMPERATURE = atof(parameterValue);		
		break;

		case PARAMATER_MAXIMUM_ACCELERATION_X:
			MAXIMUM_ACCELERATION_X = atof(parameterValue);		
		break;
		case PARAMATER_MAXIMUM_ACCELERATION_Y:
			MAXIMUM_ACCELERATION_X = atof(parameterValue);		
		break;
		case PARAMATER_MAXIMUM_ACCELERATION_Z:	
			MAXIMUM_ACCELERATION_Z = atof(parameterValue);		
		break;
		
		case PARAMATER_MINIMUM_ACCELERATION_X:	
			MINIMUM_ACCELERATION_X = atof(parameterValue);		
		break;
		case PARAMATER_MINIMUM_ACCELERATION_Y:	
			MINIMUM_ACCELERATION_Y = atof(parameterValue);		
		break;
		case PARAMATER_MINIMUM_ACCELERATION_Z:	
			MINIMUM_ACCELERATION_Z = atof(parameterValue);		
		break;
		
		
		case PARAMATER_AUTOMATIC_TEMPERATURE_TRANSMISION_PERIOD:
			AUTOMATIC_TEMPERATURE_TRANSMISION_PERIOD = atoi(parameterValue);
		break;
		case PARAMATER_AUTOMATIC_TEMPERATURE_TRANSMISION_THRESHOLD:
			AUTOMATIC_TEMPERATURE_TRANSMISION_THRESHOLD  = atof(parameterValue);
		break;
		case PARAMATER_AUTOMATIC_TOTAL_ACCELERATION_PERIOD:
			AUTOMATIC_TOTAL_ACCELERATION_PERIOD = atoi(parameterValue);		
		break;
		case PARAMATER_AUTOMATIC_TOTAL_ACCELERATION_THRESHOLD:
			AUTOMATIC_TOTAL_ACCELERATION_THRESHOLD  = atof(parameterValue);
		break;

		case PARAMATER_CURRENT_TEMPERATURE_FOR_CALIBRATION:
			TEMPERATURE_OFFSET  = atof(parameterValue) - getCelsius(analogRead(NTC_PIN));
		break;

		case PARAMATER_ACCELERATION_X_PLUS:
			ACCELERATION_X_OFFSET  += atof(parameterValue);
		break;		
		case PARAMATER_ACCELERATION_Y_PLUS:
			ACCELERATION_Y_OFFSET  += atof(parameterValue);
		break;
		case PARAMATER_ACCELERATION_Z_PLUS:
			ACCELERATION_Z_OFFSET  += atof(parameterValue);
		break;
		
		
		case PARAMATER_ACCELERATION_X_MINUS:
			ACCELERATION_X_OFFSET  -= atof(parameterValue);
		break;		
		case PARAMATER_ACCELERATION_Y_MINUS:
			ACCELERATION_Y_OFFSET  -= atof(parameterValue);
		break;
		case PARAMATER_ACCELERATION_Z_MINUS:
			ACCELERATION_Z_OFFSET  -= atof(parameterValue);
		break;

		
	}
	return valueSetSuccessfully;
}

